#include <opencv2/opencv.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <iostream>
#include <sstream>
#include <stdlib.h>
#include <unistd.h>

#include "common/common_utils/StrictMode.hpp"
STRICT_MODE_OFF
#ifndef RPCLIB_MSGPACK
#define RPCLIB_MSGPACK clmdep_msgpack
#endif // !RPCLIB_MSGPACK
#include "rpc/rpc_error.h"
STRICT_MODE_ON

#include "vehicles/car/api/CarRpcLibClient.hpp"
#include "common/common_utils/FileSystem.hpp"
#include <iostream>
#include <chrono>

#include "timer.h"

using namespace cv;
using namespace std;

void convertBRGAtoRGBA(cv::Mat mat, cv::Mat &dst) {
    for (int rowIndex = 0; rowIndex < mat.rows; ++rowIndex) {
        for (int colIndex = 0; colIndex < mat.cols; ++colIndex) {
            cv::Vec4b val = mat.at<cv::Vec4b>(rowIndex, colIndex);
            char blue = val[0];
            char green = val[1];
            char red = val[2];
            char alpha = val[3];
            //do anything you want here...
            dst.at<Vec4b>(rowIndex, colIndex)[0] = red;
            dst.at<Vec4b>(rowIndex, colIndex)[1] = green;
            dst.at<Vec4b>(rowIndex, colIndex)[2] = blue;
            dst.at<Vec4b>(rowIndex, colIndex)[3] = alpha;
        }
    }
}


int main(int argc, char* argv[])
{

    using namespace msr::airlib;

    // This assumes you are running DroneServer already on the same machine.
    // DroneServer must be running first.
    msr::airlib::CarRpcLibClient client;
    typedef ImageCaptureBase::ImageRequest ImageRequest;
    typedef ImageCaptureBase::ImageResponse ImageResponse;
    typedef ImageCaptureBase::ImageType ImageType;
    typedef common_utils::FileSystem FileSystem;

    Timer tt;

    msr::airlib::ClockBase* clock = msr::airlib::ClockFactory::get();

    try {
        client.confirmConnection();

        cv::Mat scene_img = cv::Mat(240, 320, CV_8UC4);

        for(;;)
        {
            std::vector<ImageRequest> request = { ImageRequest("front_left", ImageType::Scene,false,false)
                , ImageRequest("front_right", ImageType::Scene,false,false)
//                , ImageRequest("1", ImageType::DepthPlanner, true)
            };

            tt.tic();
            auto sample_time = clock->nowNanos();
            const std::vector<ImageResponse>& response = client.simGetImages(request);
            std::cout << "# of images received: " << response.size() << std::endl;
            tt.toc("RPCLib call simGetImages total ");

            if (response.size() > 0) {
                for (const ImageResponse& image_info : response) {
//                    std::cout << "Image uint8 size: " << image_info.image_data_uint8.size() << std::endl;
//                    std::cout << "Image float size: " << image_info.image_data_float.size() << std::endl;
//                    std::cout << "Image metrics : " << image_info.height <<"|"<<image_info.width << std::endl;
//                    std::cout << "camera_name: " << image_info.camera_name << std::endl;
                    std::cout << "--------------- sample latency "<<image_info.camera_name<<" : " << (image_info.time_stamp-sample_time) * 1e-6 << std::endl;

                    memcpy(scene_img.data,image_info.image_data_uint8.data(),image_info.image_data_uint8.size()*sizeof(unsigned char));

                    convertBRGAtoRGBA(scene_img,scene_img);
                    cv::imshow(image_info.camera_name,scene_img);
                    cv::waitKey(20);
                }
            }
        }
    }
    catch (rpc::rpc_error&  e) {
        std::string msg = e.get_error().as<std::string>();
        std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; std::cin.get();
    }

    return 0;
}





